#ifndef __ENGINE_UTILS_HPP__
#define __ENGINE_UTILS_HPP__

#include "Vector3.hpp"
#include "Matrix.h"
#include "Quaternion.h"

namespace PocketSurvival::Engine::EngineUtils
{
    inline Vector3 Transform(const Vector3 &v, const Matrix &m)
	{
		return Vector3(v.posX * m.M11 + v.posY * m.M21 + v.posZ * m.M31 + m.M41, v.posX * m.M12 + v.posY * m.M22 + v.posZ * m.M32 + m.M42, v.posX * m.M13 + v.posY * m.M23 + v.posZ * m.M33 + m.M43);
	}

	inline void Transform(const Vector3 &v, const Matrix &m, Vector3 &result)
	{
		result = Vector3(v.posX * m.M11 + v.posY * m.M21 + v.posZ * m.M31 + m.M41, v.posX * m.M12 + v.posY * m.M22 + v.posZ * m.M32 + m.M42, v.posX * m.M13 + v.posY * m.M23 + v.posZ * m.M33 + m.M43);
	}

	inline Vector3 Transform(const Vector3 &v, const Quaternion &q)
	{
		float num = q.X + q.X;
		float num2 = q.Y + q.Y;
		float num3 = q.Z + q.Z;
		float num4 = q.W * num;
		float num5 = q.W * num2;
		float num6 = q.W * num3;
		float num7 = q.X * num;
		float num8 = q.X * num2;
		float num9 = q.X * num3;
		float num10 = q.Y * num2;
		float num11 = q.Y * num3;
		float num12 = q.Z * num3;
		return Vector3(v.posX * (1.0f - num10 - num12) + v.posY * (num8 - num6) + v.posZ * (num9 + num5), v.posX * (num8 + num6) + v.posY * (1.0f - num7 - num12) + v.posZ * (num11 - num4), v.posX * (num9 - num5) + v.posY * (num11 + num4) + v.posZ * (1.0f - num7 - num10));
	}

}

#endif
